#include "Vector3.h"
#include "Math.h"
#include "Angles.h"
#include "Matrix3.h"

namespace Core
{
	rtVector3 rtVector3::vec3_origin(0.0f, 0.0f, 0.0f);
	rtVector3 rtVector3::vec3_zero(0.0f, 0.0f, 0.0f);
	
	inline rtVector3::rtVector3(void) 
	{
	}

	inline rtVector3::rtVector3(const float x, const float y, const float z) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
	}

	inline float rtVector3::operator [] (const int index) const 
	{
		return (&x)[ index ];
	}

	inline float &rtVector3::operator [] (const int index) 
	{
		return (&x)[ index ];
	}

	inline void rtVector3::set(const float x, const float y, const float z) 
	{
		this->x = x;
		this->y = y;
		this->z = z;
	}

	inline void rtVector3::zero(void) 
	{
		x = y = z = 0.0f;
	}

	inline rtVector3 rtVector3::operator - () const 
	{
		return rtVector3(-x, -y, -z);
	}

	inline rtVector3& rtVector3::operator = (const rtVector3 &a) 
	{
		x = a.x;
		y = a.y;
		z = a.z;
		return *this;
	}

	inline rtVector3 rtVector3::operator - (const rtVector3 &a) const 
	{
		return rtVector3(x - a.x, y - a.y, z - a.z);
	}

	inline float rtVector3::operator * (const rtVector3 &a) const 
	{
		return x * a.x + y * a.y + z * a.z;
	}

	inline rtVector3 rtVector3::operator * (const float a) const 
	{
		return rtVector3(x * a, y * a, z * a);
	}

	inline rtVector3 rtVector3::operator / (const float a) const 
	{
		float inva = 1.0f / a;
		return rtVector3(x * inva, y * inva, z * inva);
	}

	inline rtVector3 operator * (const float a, const rtVector3 b) 
	{
		return rtVector3(b.x * a, b.y * a, b.z * a);
	}

	inline rtVector3 rtVector3::operator + (const rtVector3 &a) const 
	{
		return rtVector3(x + a.x, y + a.y, z + a.z);
	}

	inline rtVector3& rtVector3::operator += (const rtVector3 &a) 
	{
		x += a.x;
		y += a.y;
		z += a.z;

		return *this;
	}

	inline rtVector3& rtVector3::operator /= (const rtVector3 &a) 
	{
		x /= a.x;
		y /= a.y;
		z /= a.z;

		return *this;
	}

	inline rtVector3& rtVector3::operator /= (const float a) 
	{
		float inva = 1.0f / a;
		x *= inva;
		y *= inva;
		z *= inva;

		return *this;
	}

	inline rtVector3& rtVector3::operator -= (const rtVector3 &a) 
	{
		x -= a.x;
		y -= a.y;
		z -= a.z;

		return *this;
	}

	inline rtVector3& rtVector3::operator*=(const float a) 
	{
		x *= a;
		y *= a;
		z *= a;

		return *this;
	}

	inline bool rtVector3::compare(const rtVector3& a) const 
	{
		return ((x == a.x) && (y == a.y) && (z == a.z));
	}

	inline bool rtVector3::compare(const rtVector3& a, const float epsilon) const 
	{
		if (rtMath::Fabs(x - a.x) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(y - a.y) > epsilon) 
		{
			return false;
		}

		if (rtMath::Fabs(z - a.z) > epsilon) 
		{
			return false;
		}

		return true;
	}

	inline bool rtVector3::operator == (const rtVector3& a) const 
	{
		return compare(a);
	}

	inline bool rtVector3::operator != (const rtVector3& a) const 
	{
		return !compare(a);
	}

	inline float rtVector3::normalizeFast(void) 
	{
		float sqrLength, invLength;

		sqrLength = x * x + y * y + z * z;
		invLength = rtMath::RSqrt( sqrLength );
		x *= invLength;
		y *= invLength;
		z *= invLength;
		return invLength * sqrLength;
	}

	inline bool rtVector3::fixDegenerateNormal(void) 
	{
		if (x == 0.0f) 
		{
			if (y == 0.0f) 
			{
				if (z > 0.0f) 
				{
					if (z != 1.0f) 
					{
						z = 1.0f;
						return true;
					}
				} 
				else 
				{
					if (z != -1.0f) 
					{
						z = -1.0f;
						return true;
					}
				}
				return false;
			} 
			else if (z == 0.0f) 
			{
				if (y > 0.0f) 
				{
					if (y != 1.0f) 
					{
						y = 1.0f;
						return true;
					}
				} 
				else 
				{
					if (y != -1.0f) 
					{
						y = -1.0f;
						return true;
					}
				}
				return false;
			}
		} 
		else if (y == 0.0f) 
		{
			if (z == 0.0f) 
			{
				if (x > 0.0f) 
				{
					if (x != 1.0f) 
					{
						x = 1.0f;
						return true;
					}
				}
				else 
				{
					if (x != -1.0f) 
					{
						x = -1.0f;
						return true;
					}
				}
				return false;
			}
		}
		if (rtMath::Fabs(x) == 1.0f) 
		{
			if (y != 0.0f || z != 0.0f) 
			{
				y = z = 0.0f;
				return true;
			}
			return false;
		} 
		else if (rtMath::Fabs(y) == 1.0f) 
		{
			if (x != 0.0f || z != 0.0f) 
			{
				x = z = 0.0f;
				return true;
			}
			return false;
		} 
		else if (rtMath::Fabs(z) == 1.0f) 
		{
			if (x != 0.0f || y != 0.0f) 
			{
				x = y = 0.0f;
				return true;
			}
			return false;
		}
		return false;
	}

	inline bool rtVector3::fixDenormals(void) 
	{
		bool denormal = false;
		if ( rtMath::Fabs(x) < 1e-30f ) 
		{
			x = 0.0f;
			denormal = true;
		}
		if ( rtMath::Fabs(y) < 1e-30f ) 
		{
			y = 0.0f;
			denormal = true;
		}
		if ( rtMath::Fabs(z) < 1e-30f ) 
		{
			z = 0.0f;
			denormal = true;
		}
		return denormal;
	}

	inline rtVector3 rtVector3::cross(const rtVector3 &a) const 
	{
		return rtVector3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
	}

	inline rtVector3 &rtVector3::cross(const rtVector3 &a, const rtVector3 &b) 
	{
		x = a.y * b.z - a.z * b.y;
		y = a.z * b.x - a.x * b.z;
		z = a.x * b.y - a.y * b.x;

		return *this;
	}

	inline float rtVector3::length(void) const 
	{
		return ( float )rtMath::Sqrt(x * x + y * y + z * z);
	}

	inline float rtVector3::lengthSqr(void) const 
	{
		return (x * x + y * y + z * z);
	}

	inline float rtVector3::lengthFast(void) const 
	{
		float sqrLength;

		sqrLength = x * x + y * y + z * z;
		return sqrLength * rtMath::RSqrt(sqrLength);
	}

	inline float rtVector3::normalize(void) 
	{
		float sqrLength, invLength;

		sqrLength = x * x + y * y + z * z;
		invLength = rtMath::InvSqrt(sqrLength);
		x *= invLength;
		y *= invLength;
		z *= invLength;
		return invLength * sqrLength;
	}

	inline rtVector3 &rtVector3::truncate(float length) 
	{
		float length2;
		float ilength;

		if (!length) 
		{
			zero();
		}
		else 
		{
			length2 = lengthSqr();
			if (length2 > length * length) 
			{
				ilength = length * rtMath::InvSqrt(length2);
				x *= ilength;
				y *= ilength;
				z *= ilength;
			}
		}

		return *this;
	}

	inline void rtVector3::clamp(const rtVector3& min, const rtVector3& max) 
	{
		if (x < min.x) 
		{
			x = min.x;
		} 
		else if (x > max.x) 
		{
			x = max.x;
		}
		if (y < min.y) 
		{
			y = min.y;
		} 
		else if (y > max.y) 
		{
			y = max.y;
		}
		if (z < min.z) 
		{
			z = min.z;
		} 
		else if (z > max.z) 
		{
			z = max.z;
		}
	}

	inline void rtVector3::snap(void) 
	{
		x = floor(x + 0.5f);
		y = floor(y + 0.5f);
		z = floor(z + 0.5f);
	}

	inline void rtVector3::snapInt(void) 
	{
		x = float(int(x));
		y = float(int(y));
		z = float(int(z));
	}

	inline int rtVector3::getDimension(void) const 
	{
		return 3;
	}

	inline const rtVector2& rtVector3::toVec2(void) const 
	{
		return *reinterpret_cast<const rtVector2*>(this);
	}

	inline rtVector2& rtVector3::toVec2(void) 
	{
		return *reinterpret_cast<rtVector2*>(this);
	}

	inline const float* rtVector3::toFloatPtr(void) const 
	{
		return &x;
	}

	inline float* rtVector3::toFloatPtr(void) 
	{
		return &x;
	}

	inline void rtVector3::normalVectors(rtVector3& left, rtVector3& down) const 
	{
		float d;

		d = x * x + y * y;
		if ( !d ) 
		{
			left[0] = 1;
			left[1] = 0;
			left[2] = 0;
		} 
		else 
		{
			d = rtMath::InvSqrt( d );
			left[0] = -y * d;
			left[1] = x * d;
			left[2] = 0;
		}
		down = left.cross( *this );
	}

	inline void rtVector3::orthogonalBasis(rtVector3& left, rtVector3& up) const 
	{
		float l, s;

		if (rtMath::Fabs(z) > 0.7f) 
		{
			l = y * y + z * z;
			s = rtMath::InvSqrt(l);
			up[0] = 0;
			up[1] = z * s;
			up[2] = -y * s;
			left[0] = l * s;
			left[1] = -x * up[2];
			left[2] = x * up[1];
		}
		else 
		{
			l = x * x + y * y;
			s = rtMath::InvSqrt(l);
			left[0] = -y * s;
			left[1] = x * s;
			left[2] = 0;
			up[0] = -z * left[1];
			up[1] = z * left[0];
			up[2] = l * s;
		}
	}

	inline void rtVector3::projectOntoPlane(const rtVector3& normal, const float overBounce) 
	{
		float backoff;

		backoff = *this * normal;

		if (overBounce != 1.0) 
		{
			if (backoff < 0) 
			{
				backoff *= overBounce;
			} 
			else 
			{
				backoff /= overBounce;
			}
		}

		*this -= backoff * normal;
	}

	inline bool rtVector3::projectAlongPlane(const rtVector3& normal, const float epsilon, const float overBounce) 
	{
		rtVector3 cross;
		float len;

		cross = this->cross(normal).cross((*this));
		cross.normalize();
		len = normal * cross;
		if (rtMath::Fabs( len ) < epsilon) 
		{
			return false;
		}
		cross *= overBounce * (normal * (*this)) / len;
		(*this) -= cross;
		return true;
	}

	float rtVector3::toYaw(void) const 
	{
		float yaw;

		if ((y == 0.0f) && (x == 0.0f)) 
		{
			yaw = 0.0f;
		} 
		else 
		{
			yaw = RAD2DEG(atan2(y, x));
			if (yaw < 0.0f) 
			{
				yaw += 360.0f;
			}
		}

		return yaw;
	}


	float rtVector3::toPitch(void) const 
	{
		float	forward;
		float	pitch;

		if ((x == 0.0f) && (y == 0.0f)) 
		{
			if (z > 0.0f) 
			{
				pitch = 90.0f;
			} 
			else 
			{
				pitch = 270.0f;
			}
		} 
		else 
		{
			forward = (float)rtMath::Sqrt(x * x + y * y);
			pitch = RAD2DEG(atan2(z, forward));
			if (pitch < 0.0f) 
			{
				pitch += 360.0f;
			}
		}

		return pitch;
	}


	rtAngles rtVector3::toAngles(void) const 
	{
		float forward;
		float yaw;
		float pitch;

		if ((x == 0.0f) && (y == 0.0f)) 
		{
			yaw = 0.0f;
			if (z > 0.0f) 
			{
				pitch = 90.0f;
			} 
			else 
			{
				pitch = 270.0f;
			}
		} 
		else 
		{
			yaw = RAD2DEG(atan2(y, x));
			if (yaw < 0.0f) 
			{
				yaw += 360.0f;
			}

			forward = (float)rtMath::Sqrt(x * x + y * y);
			pitch = RAD2DEG(atan2(z, forward));
			if (pitch < 0.0f) 
			{
				pitch += 360.0f;
			}
		}

		return rtAngles(-pitch, yaw, 0.0f);
	}


	rtMatrix3 rtVector3::toMat3(void) const 
	{
		rtMatrix3	mat;
		float	d;

		mat[0] = *this;
		d = x * x + y * y;
		if ( !d ) 
		{
			mat[1][0] = 1.0f;
			mat[1][1] = 0.0f;
			mat[1][2] = 0.0f;
		} 
		else 
		{
			d = rtMath::InvSqrt(d);
			mat[1][0] = -y * d;
			mat[1][1] = x * d;
			mat[1][2] = 0.0f;
		}
		mat[2] = cross(mat[1]);

		return mat;
	}


	


	void rtVector3::lerp(const rtVector3& v1, const rtVector3& v2, const float l) 
	{
		if (l <= 0.0f) 
		{
			(*this) = v1;
		} 
		else if (l >= 1.0f) 
		{
			(*this) = v2;
		} 
		else 
		{
			(*this) = v1 + l * (v2 - v1);
		}
	}


#define LERP_DELTA 1e-6

	void rtVector3::slerp(const rtVector3& v1, const rtVector3& v2, const float t) 
	{
		float omega, cosom, sinom, scale0, scale1;

		if (t <= 0.0f) 
		{
			(*this) = v1;
			return;
		} 
		else if (t >= 1.0f) 
		{
			(*this) = v2;
			return;
		}

		cosom = v1 * v2;
		if ((1.0f - cosom) > LERP_DELTA) 
		{
			omega = acos(cosom);
			sinom = sin(omega);
			scale0 = sin((1.0f - t) * omega) / sinom;
			scale1 = sin(t * omega) / sinom;
		} 
		else 
		{
			scale0 = 1.0f - t;
			scale1 = t;
		}

		(*this) = (v1 * scale0 + v2 * scale1);
	}


	void rtVector3::projectSelfOntoSphere(const float radius) 
	{
		float rsqr = radius * radius;
		float len = length();
		if (len  < rsqr * 0.5f) 
		{
			z = sqrt(rsqr - len);
		} 
		else 
		{
			z = rsqr / (2.0f * sqrt(len));
		}
	}

}

